/**
 * @file display_stage.c
 * @author nicolas.brulez@parrot.com
 * @date 2012/09/25
 *
 * This stage is a naive example of how to display video using GTK2 + Cairo
 * In a complete application, all GTK handling (gtk main thread + widgets/window creation)
 *  should NOT be handled by the video pipeline (see the Navigation linux example)
 *
 * The window will be resized according to the picture size, and should not be resized bu the user
 *  as we do not handle any gtk event except the expose-event
 *
 * This example is not intended to be a GTK/Cairo tutorial, it is only an example of how to display
 *  the AR.Drone live video feed. The GTK Thread is started here to improve the example readability
 *  (we have all the gtk-related code in one file)
 */

#include <ardrone_tool/UI/ardrone_input.h>
// Self header file
#include "display_stage.h"

// GTK/Cairo headers
#include <cairo.h>
#include <gtk/gtk.h>

#include <opencv/cv.h>
#include <opencv/highgui.h>

//#include <iostream.h>
#include <stdlib.h>
#include <stdio.h>
 
// Maths methods
#define max(a, b) ((a) > (b) ? (a) : (b))
#define min(a, b) ((a) < (b) ? (a) : (b))  
#define abs(x) ((x) > 0 ? (x) : -(x))
#define sign(x) ((x) > 0 ? 1 : -1)
 
// Step mooving for object min & max
#define STEP_MIN 2
#define STEP_MAX 100 

// constants
#define TOLERANCE_NB_PIX 10
#define TRACK_BAR_MAX_VALUE 1000
#define SPEED_LOOP_MS 50
 
// Funcs pointer definition
const vp_api_stage_funcs_t display_stage_funcs = {
    NULL,
    (vp_api_stage_open_t) display_stage_open,
    (vp_api_stage_transform_t) display_stage_transform,
    (vp_api_stage_close_t) display_stage_close
};

// Extern so we can make the ardrone_tool_exit() function (ardrone_testing_tool.c)
// return TRUE when we close the video window
extern int exit_program;

// Boolean to avoid asking redraw of a not yet created / destroyed window
bool_t gtkRunning = FALSE; 
 
IplImage *image;
//IplImage *resize;
CvFont font;

// Position of the object we overlay
CvPoint objectPos;
// Color tracked and our tolerance towards it
int h = 0, s = 0, v = 0, tolerance = 20;

// Attributs des trackBar pour le KGain et Tolerance
int trackBarValues = 100;
int *ptTrackBarValues = &trackBarValues;
int *ptTolerance = &tolerance;
 
/*
 * Transform the image into a two colored image, one color for the color we want to track, another color for the others colors
 * From this image, we get two datas : the number of pixel detected, and the center of gravity of these pixel
 */
CvPoint binarisation(IplImage* image, int *nbPixels) {
 
	int x, y;
	//CvScalar pixel;
	IplImage *hsv, *mask;
	IplConvKernel *kernel;
	int sommeX = 0, sommeY = 0;
	*nbPixels = 0;
 
	// Create the mask &initialize it to white (no color detected)
	mask = cvCreateImage(cvGetSize(image), image->depth, 1);
 
	// Create the hsv image
	hsv = cvCloneImage(image);
	cvCvtColor(image, hsv, CV_BGR2HSV);
 
	// We create the mask
	cvInRangeS(hsv, cvScalar(h - tolerance -1, s - tolerance, 0,0), cvScalar(h + tolerance -1, s + tolerance, 255,0), mask);
 
	// Create kernels for the morphological operation
	kernel = cvCreateStructuringElementEx(5, 5, 2, 2, CV_SHAPE_ELLIPSE, NULL);
 
	// Morphological opening (inverse because we have white pixels on black background)
	cvDilate(mask, mask, kernel, 1);
	cvErode(mask, mask, kernel, 1);  
 /*
	// We go through the mask to look for the tracked object and get its gravity center
	for(x = 0; x < mask->width; x++) {
		for(y = 0; y < mask->height; y++) { 
 
			// If its a tracked pixel, count it to the center of gravity's calcul
			if(((uchar *)(mask->imageData + y*mask->widthStep))[x] == 255) {
				sommeX += x;
				sommeY += y;
				(*nbPixels)++;
			}
		}
	}
 */
	// Nous prenons que la partie supérieur de l'image soit celle non parcouru
	// We go through the mask to look for the tracked object and get its gravity center
	for(x = 0; x < mask->width; x++) {
		for(y = 0; y < (mask->height)/2; y++) { 
 
			// If its a tracked pixel, count it to the center of gravity's calcul
			if(((uchar *)(mask->imageData + y*mask->widthStep))[x] == 255) {
				sommeX += x;
				sommeY += y;
				(*nbPixels)++;
			}
		}
	}	

	// Show the result of the mask image
	cvShowImage("Mask", mask);
 
	// We release the memory of kernels
	cvReleaseStructuringElement(&kernel);
 
	// We release the memory of the mask
	cvReleaseImage(&mask);
	// We release the memory of the hsv image
    	cvReleaseImage(&hsv);
 
	// If there is no pixel, we return a center outside the image, else we return the center of the line in x. y doesnt change
	if(*nbPixels > TOLERANCE_NB_PIX)
		return cvPoint((int)(sommeX / (*nbPixels)), image->height/4);// (int)(sommeY / (*nbPixels)));
	else {
		// Display STOP
		cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.4, 0.4, 0, 2, 20);
		cvPutText(image,"ARRET DU DRONE",cvPoint(50,50),&font,cvScalar(0, 0, 255, 0));
		return cvPoint(-1, -1);
	}
}
 
/*
 * Add a circle on the video that fellow your colored object
 */
void addObjectToVideo(IplImage* image, CvPoint objectNextPos, int nbPixels) {
 
	int objectNextStepX; 
	//objectNextStepY;
 
	// Calculate circle next position (if there is enough pixels)
	if (nbPixels > TOLERANCE_NB_PIX) {
 
		// Reset position if no pixel were found
		if (objectPos.x == -1 || objectPos.y == -1) {
			objectPos.x = objectNextPos.x;
			objectPos.y = objectNextPos.y;
		}
 
		// Move step by step the object position to the desired position
		if (abs(objectPos.x - objectNextPos.x) > STEP_MIN) {
			objectNextStepX = max(STEP_MIN, min(STEP_MAX, abs(objectPos.x - objectNextPos.x) / 2));
			objectPos.x += (-1) * sign(objectPos.x - objectNextPos.x) * objectNextStepX;
		}
		objectPos.y = objectNextPos.y;
/*
		if (abs(objectPos.y - objectNextPos.y) > STEP_MIN) {
			objectNextStepY = max(STEP_MIN, min(STEP_MAX, abs(objectPos.y - objectNextPos.y) / 2));
			objectPos.y += (-1) * sign(objectPos.y - objectNextPos.y) * objectNextStepY;
		}
*/
 
	// -1 = object isn't within the camera range
	} else {
 
		objectPos.x = -1;
		objectPos.y = -1;
 
	}
 
	// Draw an object (circle) centered on the calculated center of gravity
	if (nbPixels > TOLERANCE_NB_PIX)
		cvDrawCircle(image, objectPos, 5, CV_RGB(255, 0, 0), -1,8,0); 
}
 
/*
 * Get the color of the pixel where the mouse has clicked
 * We put this color as model color (the color we want to tracked)
 */
void getObjectColor(int event, int x, int y, int flags, void *param) {
 
	// Vars
	CvScalar pixel;
	IplImage *hsv;
 
	if(event == CV_EVENT_LBUTTONUP)	{
 
		// Get the hsv image
		hsv = cvCloneImage(image);
		cvCvtColor(image, hsv, CV_BGR2HSV);
 
		// Get the selected pixel
		pixel = cvGet2D(hsv, y, x);
 
		// Change the value of the tracked color with the color of the selected pixel
		h = (int)pixel.val[0];
		s = (int)pixel.val[1];
		v = (int)pixel.val[2];
 
		// Release the memory of the hsv image
    		cvReleaseImage(&hsv);
	}
}


IplImage *ipl_image_from_data(uint8_t* data, int reduced_image, int width, int height)
{
  IplImage *currframe;
  IplImage *dst;

	if (!reduced_image)
  {
    currframe = cvCreateImage(cvSize(320,240), IPL_DEPTH_8U, 3);
    dst = cvCreateImage(cvSize(320,240), IPL_DEPTH_8U, 3);
  }
  else
  {
    currframe = cvCreateImage(cvSize(176, 144), IPL_DEPTH_8U, 3);
    dst = cvCreateImage(cvSize(176,144), IPL_DEPTH_8U, 3);
    currframe->widthStep = 320*3;
  }
  //currframe = cvCreateImage(cvSize(width,height), IPL_DEPTH_8U, 3);
  //dst = cvCreateImage(cvSize(width,height), IPL_DEPTH_8U, 3);

  currframe->imageData = data;
  cvCvtColor(currframe, dst, CV_BGR2RGB);
  cvReleaseImage(&currframe);

  return dst;
}


// Picture size getter from input buffer size
// This function only works for RGB565 buffers (i.e. 2 bytes per pixel)
static void getPicSizeFromBufferSize (uint32_t bufSize, uint32_t *width, uint32_t *height)
{
    if (NULL == width || NULL == height)
    {
        return;
    }

    switch (bufSize)
    {
    case 50688: //QCIF > 176*144 *2bpp
        *width = 176;
        *height = 144;
        break;
    case 153600: //QVGA > 320*240 *2bpp
        *width = 320;
        *height = 240;
        break;
    case 460800: //360p > 640*360 *2bpp
        *width = 640;
        *height = 360;
        break;
    case 1843200: //720p > 1280*720 *2bpp
        *width = 1280;
        *height = 720;
        break;
    default:
        *width = 0;
        *height = 0;
        break;
    }
}

// Get actual frame size (without padding)
void getActualFrameSize (display_stage_cfg_t *cfg, uint32_t *width, uint32_t *height)
{
    if (NULL == cfg || NULL == width || NULL == height)
    {
        return;
    }

    *width = cfg->decoder_info->width;
    *height = cfg->decoder_info->height;
}

// Redraw function, called by GTK each time we ask for a frame redraw
static gboolean
on_expose_event (GtkWidget *widget,
                 GdkEventExpose *event,
                 gpointer data)
{
    display_stage_cfg_t *cfg = (display_stage_cfg_t *)data;

    if (2.0 != cfg->bpp)
    {
        return FALSE;
    }

    uint32_t width = 0, height = 0, stride = 0;
    getPicSizeFromBufferSize (cfg->fbSize, &width, &height);
    stride = cfg->bpp * width;

    if (0 == stride)
    {
        return FALSE;
    }

    uint32_t actual_width = 0, actual_height = 0;
    getActualFrameSize (cfg, &actual_width, &actual_height);
    gtk_window_resize (GTK_WINDOW (widget), actual_width, actual_height);

    cairo_t *cr = gdk_cairo_create (widget->window);

    cairo_surface_t *surface = cairo_image_surface_create_for_data (cfg->frameBuffer, CAIRO_FORMAT_RGB16_565, width, height, stride);

    cairo_set_source_surface (cr, surface, 0.0, 0.0);

    cairo_paint (cr);

    cairo_surface_destroy (surface);

    cairo_destroy (cr);

    return FALSE;
}

/**
 * Main GTK Thread.
 * On an actual application, this thread should be started from your app main thread, and not from a video stage
 * This thread will handle all GTK-related functions
 */
DEFINE_THREAD_ROUTINE(gtk, data)
{
    GtkWidget *window = gtk_window_new (GTK_WINDOW_TOPLEVEL);

    display_stage_cfg_t *cfg = (display_stage_cfg_t *)data;
    cfg->widget = window;

    g_signal_connect (window, "expose-event", G_CALLBACK (on_expose_event), data);
    g_signal_connect (window, "destroy", G_CALLBACK (gtk_main_quit), NULL);

    gtk_window_set_position (GTK_WINDOW (window), GTK_WIN_POS_CENTER);
    gtk_window_set_default_size (GTK_WINDOW (window), 10, 10);
    gtk_widget_set_app_paintable (window, TRUE);
    gtk_widget_set_double_buffered (window, FALSE);

    gtk_widget_show_all (window);

    gtkRunning = TRUE;

    gtk_main ();

    gtkRunning = FALSE;

    // Force ardrone_tool to close
    exit_program = 0;

    // Sometimes, ardrone_tool might not finish properly
    // This happens mainly because a thread is blocked on a syscall
    // in this case, wait 5 seconds then kill the app
    sleep (5);
    exit (0);

    return (THREAD_RET)0;
}

C_RESULT display_stage_open (display_stage_cfg_t *cfg)
{
    // Check that we use RGB565
    if (2 != cfg->bpp)
    {
        // If that's not the case, then don't display anything
        cfg->paramsOK = FALSE;
    }
    else
    {
        // Else, start GTK thread and window
        cfg->paramsOK = TRUE;
        cfg->frameBuffer = NULL;
        cfg->fbSize = 0;
        START_THREAD (gtk, cfg);
    }
    return C_OK;
}

// Function pour changer la tolerance pour le debug
void changeTolerance(int pos,void* fct) {
	tolerance = pos;
}

C_RESULT display_stage_transform (display_stage_cfg_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
	static int emerg_value = 1;
    uint32_t width = 0, height = 0;
    getPicSizeFromBufferSize(in->size, &width, &height);
    image = ipl_image_from_data((uint8_t*)in->buffers[0], 0, 360, 240);
    
    //image = cvCreateImage( cvSize((int)((resize->width*2)/100) , (int)((resize->height*2)/100) ),resize->depth, resize->nChannels );

//use cvResize to resize source to a destination image
	//cvResize(resize, image,CV_INTER_NN);
    
    //cvNamedWindow("Drone", CV_WINDOW_AUTOSIZE);
    //cvShowImage("Drone", image);

	// Number of tracked pixels
	int nbPixels;
	// Next position of the object we overlay
	CvPoint objectNextPos;
	
	// Create the windows
   	cvNamedWindow("Color Tracking", CV_WINDOW_AUTOSIZE);
   	cvNamedWindow("Mask", CV_WINDOW_AUTOSIZE);
	cvMoveWindow("Color Tracking", 0, 100);
	cvMoveWindow("Mask", 700, 100);
	cvNamedWindow("Control Panel", CV_WINDOW_AUTOSIZE);
	cvMoveWindow("Control Panel",0,500);
 
 	// Creation KGAIN trackbar, rajouter la fonction à la place de "NULL" si fonction il y a
	cvCreateTrackbar("K Gain", "Control Panel", ptTrackBarValues, TRACK_BAR_MAX_VALUE,NULL);

	// Creation TOLERANCE trackbar, rajouter la fonction à la place de "NULL"
	cvCreateTrackbar("Tolerance", "Control Panel", ptTolerance, 255,NULL);
 
	// Mouse event to select the tracked color on the original image
	cvSetMouseCallback("Color Tracking", getObjectColor, NULL);

	objectNextPos = binarisation(image, &nbPixels);
	addObjectToVideo(image, objectNextPos, nbPixels);
	
	// Draw the center of the image
		cvDrawCircle(image, cvPoint(image->width/2,image->height/2), 5, CV_RGB(0,0,255),-1,8,0);

		// Display the distance between line and center of the image
		cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.4, 0.4, 0, 1, 8);
		// Distance = centre - objectNextPos.x;
		int dist = (image->width/2) - objectNextPos.x;
		char *distTxt;
		// Affichage "Distance : %d pixels." en largeur
		distTxt = vp_os_malloc(30 * sizeof(char));
		sprintf(distTxt, "Distance : %d pixels.",dist);
		
		cvPutText(image,distTxt,cvPoint(10,10),&font,cvScalar(0, 0, 255, 0));		
		// We show the image on the window
		cvShowImage("Color Tracking", image);
 
		// Temps Boucle
		//key = cvWaitKey(SPEED_LOOP_MS);
	
	cvWaitKey(1);
    cvReleaseImage(&image);
	//bouton pour le faire décoller
	ardrone_tool_set_ui_pad_start(1);
	sleep(3);
	//bouton pour le faire atterir
	ardrone_tool_set_ui_pad_start(0);
	//bouton pour l'arrêt d'urgence
	//ardrone_tool_set_ui_pad_select(emerg_value);
	//emerg_value = (emerg_value + 1) % 2;


    return C_OK;
}

C_RESULT display_stage_close (display_stage_cfg_t *cfg)
{
    // Free all allocated memory
    if (NULL != cfg->frameBuffer)
    {
        vp_os_free (cfg->frameBuffer);
        cfg->frameBuffer = NULL;
    }

    return C_OK;
}
